﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using UnityEngine;
namespace PluginPathFinder
{
    public class PathFinder : MonoBehaviour
    {
        public GraphData graphData = new GraphData();

        private static PathFinder _instance;

        public static PathFinder instance { get { return _instance; } }

        public void Awake()
        {
            _instance = this;
        }

        public int FindNearestNode(Vector3 position)
        {
            float minDistance = float.MaxValue;
            Node nearestNode = null;
            foreach(var node in graphData.nodes)
            {
                var tmpDistance = Vector3.Distance(node.Position,position);
                if(tmpDistance < minDistance)
                {
                    nearestNode = node;
                    minDistance = tmpDistance;
                }
            }

          
            return nearestNode != null ? nearestNode.autoGeneratedID:-1;
        }

        public void FindShortestPathOfPoints(int fromNodeID,int toNodeID,System.Action<List<Node>>callback)
        {
            callback(FindShortedPathSynchronousInternal(fromNodeID, toNodeID));
        }

        private List<Node> FindShortedPathSynchronousInternal(int fromNodeID,int toNodeID)
        {
            int startPointID = fromNodeID;
            int endPointID = toNodeID;
            bool found = false;

            graphData.ReGenerateIDs();

            Node startPoint = graphData.nodesSorted[startPointID];
            Node endPoint = graphData.nodesSorted[endPointID];

            foreach (var point in graphData.nodes)
            {
                point.heuristicDistance = -1;
                point.previousNode = null;
            }

            List<Node> completedPoints = new List<Node>();
            List<Node> nextPoints = new List<Node>();
            List<Node> finalPath = new List<Node>();

            startPoint.pathDistance = 0;
            startPoint.heuristicDistance = Vector3.Distance(startPoint.Position, endPoint.Position);
            nextPoints.Add(startPoint);

            while (true)
            {
                Node leastCostPoint = null;

                float minCost = 99999;
                foreach (var point in nextPoints)
                {
                    /*
                    */
                    if (point.heuristicDistance <= 0)
                        point.heuristicDistance = Vector3.Distance(point.Position, endPoint.Position) + Vector3.Distance(point.Position, startPoint.Position);


                    if (minCost > point.combinedHeuristic)
                    {
                        leastCostPoint = point;
                        minCost = point.combinedHeuristic;
                    }
                }

                if (leastCostPoint == null)
                    break;

                if (leastCostPoint == endPoint)
                {
                    found = true;
                    Node prevPoint = leastCostPoint;
                    while (prevPoint != null)
                    {
                        finalPath.Insert(0, prevPoint);
                        prevPoint = prevPoint.previousNode;
                    }

                 

                    return finalPath;
                }

                foreach (var path in graphData.paths)
                {
                    if (path.IDOfA == leastCostPoint.autoGeneratedID
                    || path.IDOfB == leastCostPoint.autoGeneratedID)
                    {

                        if (path.isOneWay)
                        {
                            if (leastCostPoint.autoGeneratedID == path.IDOfB)
                                continue;
                        }

                        if (!path.isOpen)
                            continue;

                        Node otherPoint = path.IDOfA == leastCostPoint.autoGeneratedID ?
                                                graphData.nodesSorted[path.IDOfB] : graphData.nodesSorted[path.IDOfA];

                        if (!otherPoint.IsOpen)
                            continue;

                        if (otherPoint.heuristicDistance <= 0)
                            otherPoint.heuristicDistance = Vector3.Distance(otherPoint.Position, endPoint.Position) + Vector3.Distance(otherPoint.Position, startPoint.Position);

                        if (completedPoints.Contains(otherPoint))
                            continue;

                        if (nextPoints.Contains(otherPoint))
                        {
                            if (otherPoint.pathDistance >
                                (leastCostPoint.pathDistance + path.cost))
                            {
                                otherPoint.pathDistance = leastCostPoint.pathDistance + path.cost;
                                otherPoint.previousNode = leastCostPoint;
                            }
                        }
                        else
                        {
                            otherPoint.pathDistance = leastCostPoint.pathDistance + path.cost;
                            otherPoint.previousNode = leastCostPoint;
                            nextPoints.Add(otherPoint);
                        }
                    }
                }

                nextPoints.Remove(leastCostPoint);
                completedPoints.Add(leastCostPoint);
            }

            if (!found)
            {
                return null;
            }

            return null;
        }
    }
}
